#include "service.h"
#include "cordic.h"
#include "Lowpass_Filter.h"
#include "pid.h"
#include "gpio.h"
#include "foc.h"
#include "string.h"
#include "adc.h"
#include "abz_encoder.h"
#include "pwm.h"
#include "hall_sensor.h"
#include "motor.h"

TaskHandle_t focTaskHandle = NULL;
TaskHandle_t pidTaskHandle = NULL;
TaskHandle_t monitorTaskHandle = NULL;


struct LOWPASS M_VEL_Filter = {0.1};

PID M_SPEED_PID = {0};
PID M_POSITION_PID = {0};
CascadePID M_PID = {0};

// float position_target = 0.0f;

// float motor_target = 0.0f;

// float Iq_target = 0.0f;

// MotorModes global_mode = MOTOR_MODE_STOP;
// MotorModes global_mode = MOTOR_MODE_POSITION;

uint32_t focTaskCounter = 0; // 初始化计数器
uint32_t pidTaskCounter = 0; // 初始化计数器
uint32_t canReceiveCounter = 0; // 初始化计数器
uint32_t uartReceiveCounter = 0; // 初始化计数器
TickType_t lastWakeTime = 0; // 初始化上次记录的时间
void foc_task(void *params)
{
  // 获取当前时间作为初始时间
  lastWakeTime = xTaskGetTickCount();
  while (Hall_Angle() == -1)
  {
    LED7_ON();
    // 霍尔传感器故障
    osDelay(10);
  }
  LED7_OFF();
  // 使能pwm输出
  PWM_En();

  // 辨识零点电角度   
//  while (!encoder_angle_valid)
//  {
//    osDelay(10);
//  }
//  
//  Check_Sensor();

  while (1)
  {
    // 等待任务通知
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
    focTaskCounter++; // 增加计数器

    // 计算母线电压
    calVbus(adc_Vbus);  

    // 计算三相电流
    calCurrentUVW(adc_U, adc_V);
    // printf("adc_Vbus: %d, adc_U: %d, adc_V: %d\n", adc_Vbus, adc_U, adc_V);
    // printf("VBUS: %f, CURRENT_U: %f, CURRENT_V: %f, CURRENT_W: %f\n", vbus, current_u, current_v, current_w);
 
    // Motor_Run();
    // Set_CurTorque(Iq_target);

    // SetDCVoltage(1.3f);
    Set_CurTorque(motor_target.loop_output.torque);


  //   // 获取当前时间
  //   TickType_t currentTime = xTaskGetTickCount();
  //   // 判断是否已经过去了 1 秒
  //   if ((currentTime - lastWakeTime) >= pdMS_TO_TICKS(1000))
  //   {
  //     // 打印计数器值
  //     printf("foc_task executed %u times in the last second\n", focTaskCounter);
  //     // Calculate_Sine(1.57f);
  //     // Calculate_Sine(3.14f);
      
  //     // printf("Iq: %f, Id: %f\n", I_q, I_d);
  //     focTaskCounter = 0; // 重置计数器
  //     lastWakeTime = currentTime; // 更新上次记录的时间
  //   }
  }
  
}

void pid_task(void *params)
{
  TickType_t xLastWakeTime;
  const TickType_t xFrequency = pdMS_TO_TICKS(1);

  xLastWakeTime = xTaskGetTickCount();

  // 位置环参数
  M_POSITION_PID.kp = 5.0f;  // 位置环 p
  M_POSITION_PID.ki = 0.1f; // 位置环 i
  M_POSITION_PID.kd = 0.0f;   // 位置环 d
  M_POSITION_PID.maxIntegral = 10;  // 积分限幅
  M_POSITION_PID.maxOutput = 100;   // 输出限幅，最大速度100

  // 速度环参数
  M_SPEED_PID.kp = 0.2f;  // 速度环 p
  M_SPEED_PID.ki = 0.0005f;  // 速度环 i
  M_SPEED_PID.kd = 0.0f;  // 速度环 d
  M_SPEED_PID.maxIntegral = 10; // 积分限幅
  M_SPEED_PID.maxOutput = 4.0;  // 输出限幅，最大电压1.5

  M_PID.inner = M_SPEED_PID;
  M_PID.outer = M_POSITION_PID;

  // while (!abs_encoder.enable)
  // {
  //   osDelay(100);
  // }
  

  for (;;)
  {
    pidTaskCounter++;
    speed_encoder = Lowpassfilter(&M_VEL_Filter, Cal_Encoder_Speed());
    // printf("speed_encoder:%f\n", speed_encoder);
    if (speed_encoder > 0)
    {
      LED9_ON();
      LED10_OFF();
    } else if (speed_encoder < 0)
    {
      LED10_ON();
      LED9_OFF();
    }
    
    // speed_encoder = Cal_Encoder_Speed();

    // 速度控制
    // PID_Calc(&M_SPEED_PID, -5, speed_encoder);
    // if (M_SPEED_PID.output > 1.5)
    // {
    //   Vd = 1.5;
    // } else if (M_SPEED_PID.output < -1.5)
    // {
    //   Vd = -1.5;
    // } else
    // {
    //   Vd = M_SPEED_PID.output;
    // }

    // 计算0到2π的往复运动，1秒1个周期
    // float time = (float)xTaskGetTickCount() / configTICK_RATE_HZ;
    // float position_ref = (sin(2 * PI * time) + 1) * PI;  // 0到2π的往复运动

    // 位置控制
    PID_CascadeCalc(&M_PID, motor_target.user_target.position, abs_encoder.position, speed_encoder);
    if (motor_target.work_mode == MOTOR_MODE_STOP) // 停止模式
    {
      motor_target.ctrl_mode = MOTOR_CTRL_OPEN_LOOP;  // 开环控制
      motor_target.user_target.voltage_d = 0.0f;
    } else if (motor_target.work_mode == MOTOR_MODE_TORQUE)  // 力矩模式
    {
      motor_target.ctrl_mode = MOTOR_CTRL_CLOSED_LOOP;  // 闭环控制
      // motor_target.user_target.current_q = motor_target;
    }
     else if (motor_target.work_mode == MOTOR_MODE_POSITION) // 位置模式
    {
      if (M_PID.output > 3.0f)
      {
        motor_target.loop_output.torque = 3.0f;
      } else if (M_PID.output < -3.0f)
      {
        motor_target.loop_output.torque = -3.0f;
      } else {
        motor_target.loop_output.torque = M_PID.output;
      }
    }

    
    // 通过CAN发送位置信息
    uint8_t positionData[4];
    memcpy(positionData, &abs_encoder.position, sizeof(position_encoder));
    // FDCAN_SendData(LOCAL_ID, positionData, sizeof(positionData));

    // Vd = 0.0f;
    vTaskDelayUntil(&xLastWakeTime, xFrequency);
  } 
}

void monitor_task(void *params)
{
  TickType_t lastMonitorTime = xTaskGetTickCount();
  const TickType_t monitorFrequency = pdMS_TO_TICKS(1000);

  for (;;)
  {
    vTaskDelayUntil(&lastMonitorTime, monitorFrequency);

    // 打印计数器值
    // printf("foc_task speed %u hz\n", focTaskCounter);
    // printf("pid_task speed %u hz\n", pidTaskCounter);
    // printf("can_receive speed %u hz\n", canReceiveCounter);
    // printf("485 receive speed %u hz\n", uartReceiveCounter);
    // printf("VBUS: %f, CURRENT_U: %f, CURRENT_V: %f, CURRENT_W: %f\n", vbus, current_u, current_v, current_w);

    // printf("%f,%f,%f,%f,%f,%f,%f\n",current_u, current_v, current_w, I_q, I_d, Vd,electricAngle());
    // printf("Hall Sensor angle: %f, encode angle: %f\n", Hall_Angle(), sensor_angle);
    // printf("Vd: %f, Iq_target: %f I_q: %f, I_d: %f, speed :%f, abz_position:%f\n", Vd, Iq_target, I_q, I_d, speed_encoder, position_encoder);
    // printf("target_position: %f\n", position_target);
                 // printf("%d,%d,%d\n", adc_Vbus, adc_U, adc_V);
		// printf("%x\n", LOCAL_ID);
    // uint8_t data[8] = {0x0ee, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x09};
    // FDCAN_SendData(0x100, data, 8);
    // printf("speed: %f\n", speed_encoder);
    // printf("position: %f, abs_position: %f\n", position_encoder, abs_encoder.position);
    // printf("%f\n", I_q);

    printf("position: %f\n", position_encoder);
    // 重置计数器
    focTaskCounter = 0;
    pidTaskCounter = 0;
    canReceiveCounter = 0;
    uartReceiveCounter = 0;
  }
}
